//
// URControllers.cpp
//
//
// Copyright (c) 2017 Zhou Yang @ BICE (bicezhou@163.com)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "ur_sdk.h"
#include "data_logger.h"
#include "joint_space_control.h"

#define CYCLIC_TIME_MS 8

boost::condition_variable cRun;
boost::mutex mUpdate;

void run(controllers::Joint_Space_Control* joint_space_control)
{
	boost::mutex::scoped_lock sl(mUpdate);
	cRun.wait(sl);
	//boost::posix_time::ptime start_time_ = boost::get_system_time();
	joint_space_control->starting(boost::get_system_time());
	while(true)
	{
		joint_space_control->update(boost::get_system_time(), boost::posix_time::microseconds(CYCLIC_TIME_MS));
		//boost::this_thread::sleep(boost::posix_time::milliseconds(8));
		cRun.wait(sl);
	}
	joint_space_control->stopping(boost::get_system_time());
}

void logger(controllers::Data_Logger* data_logger, controllers::Joint_Space_Control* joint_space_control)
{
	boost::mutex::scoped_lock sl(mUpdate);
	cRun.wait(sl);
	data_logger->starting(boost::get_system_time());
	joint_space_control->log_start(boost::get_system_time());
	while(true)
	{
		data_logger->update(boost::get_system_time(), boost::posix_time::microseconds(CYCLIC_TIME_MS));
		joint_space_control->log_update(boost::get_system_time(), boost::posix_time::microseconds(CYCLIC_TIME_MS));
		cRun.wait(sl);
	}
	data_logger->stopping(boost::get_system_time());
	joint_space_control->log_stop(boost::get_system_time());
}

int main(int argc, char* argv[])
{
	SetPriorityClass(GetCurrentProcess(),REALTIME_PRIORITY_CLASS);
	try
	{
		// Check command line arguments.
		if (argc != 3)
		{
			std::cerr << "Usage: client <host> <port>" << std::endl;
			return 1;
		}
		
		boost::asio::io_service io_service;
		boost::asio::io_service::work work(io_service);
		ur_sdk::client client(io_service, cRun, argv[1], argv[2]);
		controllers::Data_Logger data_logger(client);
		controllers::Joint_Space_Control joint_space_control(client);

		data_logger.init("./ur.dat");
		joint_space_control.init(boost::get_system_time());
		joint_space_control.log_init("./joint_space_control.dat");

		//boost::thread_group thr_grp;
		//thr_grp.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));
		//thr_grp.join_all();

		boost::thread thread_io_service(boost::bind(&boost::asio::io_service::run, &io_service));
		boost::thread thread_logger(boost::bind(&logger, &data_logger, &joint_space_control));
		boost::thread thread_run(boost::bind(&run, &joint_space_control));

		SetThreadPriority(thread_io_service.native_handle(),THREAD_PRIORITY_TIME_CRITICAL);
		SetThreadPriority(thread_run.native_handle(),THREAD_PRIORITY_TIME_CRITICAL);

		thread_io_service.join();
		thread_run.join();
		thread_logger.join();
	}
	catch (std::exception& e)
	{
		std::cerr << e.what() << std::endl;
	}

	return 0;
}